#include <ros/ros.h>
#include <serial/serial.h>
#include <std_msgs/String.h>
#include <xfm_localization/xfm_localization.h>
#include <iostream>
#include <map>

static serial::Serial ser;
static std::string result;

void xmfCmd(const std_msgs::String::ConstPtr& msg)
{
  /*waring:
  CMD:VER\n
  CMD:RESET\n
  CMD:BEAM []\n   usage: BEAM num[0~3]
  CMD:CALL_MODE []\n   usage: CALL_MODE num[0-1]
  CMD:WAKEUP []\n   usage: WAKEUP num[0-1]
  */
  ser.write(msg->data+"\n");
  std::string readBuffer;
  ser.readline(readBuffer);
  std::vector<std::string> lineString =  ser.readlines();

  ROS_INFO_STREAM(readBuffer);
//  for(int i=0;i<lineString.size();i++)
//  {
//    std::cout<<lineString[i];
//  }
}
//2X3
xfm_localization::xfm_localization splitString(const std::string &source)
{
  std::multimap<std::string,int> v;
  std::string::size_type pos1,pos2,pos3;
  pos1 = source.find("angle:");
  pos2 = source.find("score:");
  pos3 = source.find("key_word:");
  std::string angle =source.substr(pos1,pos2-pos1);
  std::string score =source.substr(pos2,pos3-pos2);
  std::string key_word =source.substr(pos3,key_word.find_first_of("#")-pos3);
  angle = angle.substr(angle.find(":")+1);
  score = score.substr(score.find(":")+1);
  key_word = key_word.substr(key_word.find(":")+1,16);
  // erase space
  key_word = key_word.erase(0,key_word.find_first_of(" ")+1);
  key_word = key_word.erase(key_word.find_last_not_of(" ")+1);

  std::cout<<"------xfm10521 data-----"<<std::endl;
  std::cout<<"  angle     : "<<angle<<std::endl;
  std::cout<<"  score     : "<<score<<std::endl;
  std::cout<<"  key_word  : "<<key_word<<std::endl;

  xfm_localization::xfm_localization msg;
  msg.angle = atoi(angle.c_str());
  msg.score = atoi(score.c_str());
  msg.key_word = key_word;


  return msg;
}

int main(int argc,char** argv)
{
  try
  {
    ser.setPort("/dev/ttyUSB0");
    ser.setBaudrate(115200);
    serial::Timeout to = serial::Timeout::simpleTimeout(1000);
    ser.setTimeout(to);
    ser.open();
  }
  catch(serial::IOException& e)
  {
    ROS_ERROR_STREAM("unable to open port");
    return -1;
  }
  if(ser.isOpen())
  {
    ROS_INFO_STREAM("serial port initialized");
  }
  else
  {
    return -1;
  }

  //std::cout<<"avilable:"<<ser.available()<<std::endl;
  ros::init(argc,argv,"xfm_drive");
  ros::NodeHandle xfm_drive_nh;

  ros::Publisher pub_topic = xfm_drive_nh.advertise<xfm_localization::xfm_localization>("xfm_drive_data",1000);
  ros::Publisher pub_topic_cmd = xfm_drive_nh.advertise<std_msgs::String>("xfm_drive",1000);
  ros::Subscriber set_cmd = xfm_drive_nh.subscribe("xfm_drive",1000,xmfCmd);

  ros::Rate loop_rate(50);
  while(ros::ok())
  {
    if(ser.available())
    {
      ROS_INFO_STREAM("Reading from serial port\n");
      result= ser.read(ser.available());
      if(result.find("angle:")!=std::string::npos)
      {
        xfm_localization::xfm_localization msg = splitString(result);
        pub_topic.publish(msg);
        std::cout<<"------send    over-----"<<std::endl;
      }

    }
    ros::spinOnce();
    loop_rate.sleep();
  }

}
